/*
 * obstacleAvoidance.cpp
 *
 *  Created on: Nov 13, 2012
 *      Author: Matthijs
 */
#include <behavior/obstacleAvoidance.h>


ObstacleAvoidance::ObstacleAvoidance (ros::NodeHandle n) :
	nodeHandle(n),
	frontLeft(0),
	frontRight(0)
{
	priority 	= 0.1;
	active 		= true;
	subscriber 	= nodeHandle.subscribe("/sensor/ir", 5, & ObstacleAvoidance::distance_callback, this);

	ROS_INFO("Obstacle avoidance behavior ready.");
}


/**
 * 	Parse information about obstacles in front of us, and adjust priority accordingly.
 *
 * 	If there is something close and up front, give obstacle avoidance the highest
 * 	priority possible. Otherwise, lay dorment.
 */
void ObstacleAvoidance::distance_callback (const sensor::IR::ConstPtr & msg)
{
	leftFront 	= msg -> dists[0];
	leftBack 	= msg -> dists[1];
	frontRight  = msg -> dists[6];
	frontLeft	= msg -> dists[7];
	if ((frontLeft  < 0.25 && frontLeft  > 0.01) ||
		(frontRight < 0.25 && frontRight > 0.01))
		priority = 1.0;
	else
		priority = 0.1;
}


/**
 * 	Obstacle Avoidance: ACT
 *
 * 	This method ACTS to AVOID OBSTACLES. It will attempt to turn for roughly
 * 	ten seconds, on the spot, so if it was facing a certain direction, it will
 * 	turn by 90 degrees without interruptions.
 */
bool ObstacleAvoidance::act (ros::Publisher publisher)
{
	ROS_INFO("Avoiding obstacles.");

	driver::Move move;

	// We see something up front: only rotate for the next 10 steps (~ 2 seconds).
	move.distance 				= 0.0;
	move.wait_until_completion	= true;
	//if (leftFront > leftBack)
		move.angle  =  -1.52;
	//else
		//move.angle 	= -1.70;

	ROS_INFO("AVOIDING WALL AHEAD: %f, %f", move.distance, move.angle);
	publisher.publish(move);
	return true;
}
